home *** CD-ROM | disk | FTP | other *** search
- // CARZ.CPP - simulated robot car racing - begin development Dec. '94
- // by Mitchell E. Timin, State College, PA
- // see CAR.H & TRACK.H for class and structure declarations
- // This version is for Borland C++, version 3.1, and is for DOS
- // This is part of version 0.39 of RARS (Robot Auto Racing Simulation)
- // ver. 0.1 release January 12, 1995
- // ver. 0.2 1/23/95
- // ver. 0.3 2/7/95
- // ver. 0.39 3/6/95
-
- #include <math.h>
- #include <ctype.h>
- #include <stdlib.h>
- #include <string.h>
- #include <iostream.h>
- #include "car.h"
- #include "track.h"
- #include "os.h"
-
- static const double PM = 1e5; // Power, Maximum, 100,000 ft. lb. per sec
- static const double g = 32.2; // acceleration due to gravity, ft./sec^2
- static const double DRAG_CON = .0065; // air drag, lb. per (ft/sec)^2
- static const double M = 80; // mass, slugs (2600 lb.)
- static const double STARTING_SPEED = 40.0; // cars start at this speed, ft/sec
- static const double REVERSE_GEAR_LIMIT = 20; // ft/sec max to allow reverse
-
- // global variables:
- // These are set by the command line arguments: (see get_args())
- int lap_count; // length of the race in laps
- int car_count; // how many cars in the race
- int race_count; // how many races?
- int real_speed; // if non-zero, PC clock will control speed of race
- int no_display; // if non-zero, race will be invisible
- int keep_order; // if this is 0, then starting order will be re-arranged
-
- char *glob_name; // will hold name of each robot driver (one at a time)
- double length; // total lenth of track (average of inner and outer rails)
- int done_count; // incremented by each car that finishes the race
- int out_count; // incremented by each car that crashes
- double time_count; // elapsed time, seconds
-
- // These are the control or "driver" programs which compete in the race:
- con_vec cntrl0(situation);
- con_vec cntrl1(situation); // See drivers[], below.
- con_vec cntrl2(situation);
- con_vec cntrl3(situation);
- con_vec cntrl4(situation);
- con_vec cntrl5(situation);
- con_vec cntrlb0(situation);
- con_vec cntrlb(situation);
- con_vec cntrlR(situation);
- con_vec bill(situation);
- con_vec dynamic(situation);
- con_vec arelys(situation);
- con_vec Ramdu2(situation);
-
- Car* pcar[MAXCARS]; // array of pointers to the various cars
- char* nam_ptr[MAXCARS]; // array of pointers to their name strings
- char* namptr[MAXCARS]; // re-arrangement of nam_ptr[]
- int* order; // will point to array of positions
- int* new_data; // points to array of new data indicators
- int lap[MAXCARS]; // set, then cleared as each lap completes
- colors car_colors[MAXCARS]; // There should be MAXCARS of these color pairs:
-
- // This is the permanent array of available drivers.
- // Their order here determines their car colors.
- con_vec(*drovers[])(situation) = { // pointers to their control programs
- cntrlR, Ramdu2, cntrl1, cntrl2, cntrlb0, arelys,
- cntrl3, cntrlb, cntrl0, cntrl4, cntrl5, bill,
- dynamic, dynamic };
-
- // This is the array used during the race. (It will be re-arranged.)
- // The order here determines their starting positions.
- con_vec(*drivers[])(situation) = {
- cntrlR, Ramdu2, cntrl1, cntrl2, cntrlb0, arelys,
- cntrl3, cntrlb, cntrl0, cntrl4, cntrl5, bill,
- dynamic, dynamic };
-
- void report_overall(int, int, char**); // in REPORT.CPP
- void report_results(int, int*, char**, Car**); // in REPORT.CPP
- void RAM_report(void); // in REPORT.CPP
-
- // these things are defined in DRAW.CPP:
- extern int xcon(double x); // convert x coordinate to pixels
- extern int ycon(double y); // convert y coordinate to pixels
- extern int round(double given); // convert double to int by rounding
- extern void draw_arc(double,double,double,double,double);
- extern void make_dec_string(char* out, double input);
- extern void get_avg_spd(int i, char* out);
- extern void get_max_spd(int i, char* out);
- extern double drawpath(double, double, double, segment*);
- extern int drawcar(double, double, double, int, int);
- extern void lapper(int which, int lap); // shows lap count on scoreboard,
- extern void resume_normal_display(void);
- extern void leaders(int);
- extern void scoreboard(void);
- extern void graph_setup(void);
- extern void refresh_finish_line(void);
-
- // call repeatedly to change direction by 180 degrees:
- inline void reverse(double v, double* alpha_ptr, double* vc_ptr)
- {
- if(v > 10.0) // This is algorithm to reverse velocity vector:
- *vc_ptr = 0.0; // if not going very slow, brake hard
- else
- *vc_ptr = -15.0; // when going slow enough, put 'er in reverse!
- *alpha_ptr = 0.0; // don't turn.
- }
-
- /* This routine analyses the parameters to determine if the car is in an
- abnormal situation. If so, it returns non-zero & sets the result
- vector so as to free the car. If all is normal it returns zero.
- This is a service for the robot drivers; it is only called by them. */
- int stuck(int backward, double v, double vn, double to_lft,
- double to_rgt, double* alpha_ptr, double* vc_ptr)
- {
- if(to_lft < 0.0) // If over the left wall,
- if(vn > .5 * v) { // test for more than 30 degrees off course
- reverse(v, alpha_ptr, vc_ptr);
- return 1;
- }
- else if(vn > -.5 * v && backward) { // or going well backward
- reverse(v, alpha_ptr, vc_ptr);
- return 1;
- }
- else if(vn < -.5 * v) { // heading away from wall,
- *alpha_ptr = .03; // turn to left
- *vc_ptr = (.66667 * v + 10.0); // accelerate toward 30 fps
- return 1;
- }
- else {
- *alpha_ptr = -.03; // turn to right
- *vc_ptr = (.66667 * v + 10.0); // accelerate toward 30 fps
- return 1;
- }
- else if(to_rgt < 0.0) // if over the right wall:
- if(vn < -.5 * v) { // test for more than 30 degrees off course
- reverse(v, alpha_ptr, vc_ptr);
- return 1;
- }
- else if(vn < .5 * v && backward) { // or going well backward
- reverse(v, alpha_ptr, vc_ptr);
- return 1;
- }
- else if(vn > .5 * v) { // heading away from wall,
- *alpha_ptr = -.03; // turn to right
- *vc_ptr = .66667 * v + 10.0; // accelerate toward 30 fps
- return 1;
- }
- else {
- *alpha_ptr = .03; // turn to left
- *vc_ptr = .66667 * v + 10.0; // accelerate toward 30 fps
- return 1;
- }
- else if(backward)
- if(vn > .866 * v) { // you are going more-or-less sideways left
- *alpha_ptr = -.03;
- *vc_ptr = .66667 * v + 10;
- return 1;
- }
- else if(vn < -.866 * v) { // you are going more-or-less sideways rt.
- *alpha_ptr = .03;
- *vc_ptr = .66667 * v + 10;
- return 1;
- }
- else {
- reverse(v, alpha_ptr, vc_ptr);
- return 1;
- }
- else if(v < 15) { // nothing wrong except you are going very slow:
- if(to_rgt > to_lft) // you are on left side of track
- if(vn < -.7 * v) // and you are not heading very much to right
- *alpha_ptr = -.03;
- else
- *alpha_ptr = .03;
- else // you are on the right side,
- if(vn > .7 * v) // and you are not heading very much to left
- *alpha_ptr = .03;
- else
- *alpha_ptr = -.03;
- *vc_ptr = .66667 * v + 10; // acellerate moderately
- return 1;
- }
- return 0; // We get here only if all is normal.
- }
-
- // a sorting routine that's fast when things are already in order:
- // This routine updates the order[] array which has the position of each
- // car. order[0] is the ID of the leader, order[1] is in second place, etc.
- // Also, the new_data[] array is set to show which positions have changed.
- void sortem(int num_disp)
- {
- int i, temp;
- int old_order[MAXCARS];
-
- for(i=0; i<num_disp; i++) // copy first part of order[] array
- old_order[i] = order[i];
-
- for(i=0; i<car_count-1; i++) // the while loop below does not usually repeat:
- while(farther(pcar[order[i]], pcar[order[i+1]])) {
- temp = order[i]; // When a car passes another, we swap positions
- order[i] = order[i+1];
- order[i+1] = temp;
- if(i > 0) // now we have to see if it passed another car
- --i;
- }
-
- for(i=0; i<num_disp; i++) // see what has changed and flag it:
- new_data[i] = (old_order[i] != order[i]);
- }
-
- inline double vec_mag(double x, double y) // sqrt of sum of squares
- { // (used for vector magnitude)
- return sqrt(x * x + y * y);
- }
-
- // Comparison routine for determining who's ahead: (i.e., sorting)
- // Returns 0 iff car0 has gone farther than car1, 1 otherwise.
- int farther(Car* car0, Car* car1)
- {
- int blok0, blok1; /* A block is the same as a track segment except for
- segment 0. For segment 0, the block is 0 when the
- car is past the finish line. When the car is heading
- toward the finish line the block is NSEG. */
- blok0 = car0->seg_id;
- if(!blok0)
- if(car0->to_end > from_start_to_seg1)
- blok0 = NSEG;
- blok1 = car1->seg_id;
- if(!blok1)
- if(car1->to_end > from_start_to_seg1)
- blok1 = NSEG;
- if(car0->laps > car1->laps)
- return 0;
- else if(car0->laps < car1->laps)
- return 1; // else laps are equal
- else if(blok0 > blok1)
- return 0;
- else if(blok1 > blok0)
- return 1; // else they are in the same blok:
- else if(car0->to_end <= car1->to_end)
- return 0;
- else return 1; // returns 1 in case of total equality
- }
-
- inline int incseg(int seg) // returns the next segment of the track
- { // (i.e. 0 yields 1, 1 yields 2, but
- if(++seg == NSEG) // NSEG-1 yields 0)
- seg = 0;
- return seg;
- }
-
- // This is the model of the track friction force on the tire. This force
- // provides propulsion, cornering, and braking. Force is assumed to depend
- // only on the slip velocity, rising very rapidly with small slip velocity,
- // and then asymtotically approaching an upper limit. (This is similar to
- // tires on unpaved surfaces.) ALSO: There is randomness in the force at
- // small slip velocities!
- const double MYU_MAX = 1.0; // maximum coeficient of friction, tire vs. track:
- inline double rfriction(double slip) // returns the coef. of friction,
- { // given the slip speed, ft. per sec.
- double slipping; // slip speed at which half maximum force is reached
-
- slipping = 1.5 + (double)rand()/RAND_MAX; // range is 1.5 to 2.5 fps
- return (MYU_MAX * slip)/(slipping + slip);
- }
-
- // This is a version of the above without any randomness. They produce the
- // same result when the random variable in rfriction() has its mean value.
- const double SLIPPING = 2.0;
- inline double friction(double slip) // returns the coef. of friction,
- { // given the slip speed, ft. per sec.
- return (MYU_MAX * slip)/(SLIPPING + slip);
- }
-
- // Member Functions of the Car Class:
-
- // returns laps and passes seg_id and distance to next segment via pointers:
- inline int Car::where_is_it(int* seg_id_addr, double* to_end_addr)
- {
- *seg_id_addr = seg_id;
- *to_end_addr = to_end;
- return laps;
- }
-
- // limits the rate of change and maximum value of the angle of attack:
- double alpha_limit(double was, // This is what alpha was
- double request) // The robot wants this alpha
- {
- const double MAX_RATE = 4.6; // maximum radians per second possible
- const double MAX_ALPHA= 1.0; // maximum radians possible
-
- double alpha; // the result to return
-
- if(request - was > MAX_RATE * delta_time) // want more positive alpha
- alpha = was + MAX_RATE * delta_time;
- else if(was - request > MAX_RATE * delta_time) // want more negative alpha
- alpha = was - MAX_RATE * delta_time;
- else
- alpha = request;
-
- if(alpha > MAX_ALPHA)
- alpha = MAX_ALPHA;
- else if(alpha < -MAX_ALPHA)
- alpha = -MAX_ALPHA;
-
- return alpha;
- }
-
- // the purpose of control() is to call the car's driver function, and
- // then to return the steering and throttle settings produced by
- // that routine.
- void Car::control(situation s) // calls the control function via the pointer
- { // that was installed in the car object by
- con_vec output; // by the constructor.
- con_vec (*func_ptr)(situation);
-
- if(out)
- output.alpha = output.vc = 0.0; // no action if out of race
- else {
- s.data_ptr = data_ptr;
- s.starting = starting; // startup signal for the robot
- starting = 0;
- func_ptr = cntrl;
- output = (*func_ptr)(s); // call the robot driver to set alpha and vc
- }
- alpha = output.alpha; vc = output.vc; // set private vars in car object
- }
-
- double power_excess(double vc, double sine, double cosine, double v)
- {
- double Ln, Lt; // normal and tangential components of slip vector
- double l; // magnitude of slip vector, ft. per sec.
- double F; // force on car from track, lb.
- double Fn, Ft; // tangential and normal (to car's path) force components
-
- Ln = -vc * sine; Lt = v - vc * cosine; // vector sum to compute slip vector
- l = vec_mag(Lt, Ln); // compute slip speed
- F = M * g * friction(l); // compute friction force from track
- if(l < .0001) // to prevent possible division by zero
- Fn = Ft = 0.0;
- else {
- Fn = -F * Ln/l; // compute components of force vector
- Ft = -F * Lt/l;
- }
- // compute power delivered:
- return (vc < 0.0 ? -vc : vc) * (Ft * cosine + Fn * sine) - .9975*PM;
- }
-
- inline double abs(double x)
- {
- if(x < 0.0)
- return -x;
- else
- return x;
- }
-
- inline double SIGN(double a, double b)
- {
- return b >= 0.0 ? abs(a) : -abs(a);
- }
-
- // This routine was adapted from the book "Numerical Recipes in C" by
- // Press, et. al. It searches for a value of vc which causes the
- // power to be very close to the maximum available.
- // (This is Brent's method of root finding.) x1 and x2 are values of
- // vc which bracket the root. b represents the variable vc.
- double zbrent(double sine, double cosine, double v, double x1, double x2, double tol)
- {
- const int ITMAX = 20;
- const double EPS = 1.0e-8;
- int iter;
- double a=x1, b=x2, c=x2, d,e,min1,min2;
- double fa=power_excess(a, sine, cosine, v);
- double fb=power_excess(b, sine, cosine, v);
- double fc,p,q,r,s,tol1,xm;
- double Ln, Lt; // normal and tangential components of slip vector
- double l; // magnitude of slip vector, ft. per sec.
- double F; // force on car from track, lb.
- double Fn, Ft; // tangential and normal (to car's path) force components
-
- if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0))
- return b; // This should never happen.
- fc=fb;
- for (iter=1;iter<=ITMAX;iter++) {
- if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)) {
- c=a;
- fc=fa;
- e=d=b-a;
- }
- if (abs(fc) < abs(fb)) {
- a=b;
- b=c;
- c=a;
- fa=fb;
- fb=fc;
- fc=fa;
- }
- tol1=2.0*EPS*abs(b)+0.5*tol;
- xm=0.5*(c-b);
- if (abs(xm) <= tol1 || fb == 0.0)
- return b;
- if (abs(e) >= tol1 && abs(fa) > abs(fb)) {
- s=fb/fa;
- if (a == c) {
- p=2.0*xm*s;
- q=1.0-s;
- } else {
- q=fa/fc;
- r=fb/fc;
- p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
- q=(q-1.0)*(r-1.0)*(s-1.0);
- }
- if (p > 0.0) q = -q;
- p=abs(p);
- min1=3.0*xm*q-abs(tol1*q);
- min2=abs(e*q);
- if (2.0*p < (min1 < min2 ? min1 : min2)) {
- e=d;
- d=p/q;
- } else {
- d=xm;
- e=d;
- }
- } else {
- d=xm;
- e=d;
- }
- a=b;
- fa=fb;
- if (abs(d) > tol1)
- b += d;
- else
- b += SIGN(tol1,xm);
- Ln = -b * sine; Lt = v - b * cosine; // vector sum to compute slip vector
- l = vec_mag(Lt, Ln); // compute slip speed
- F = M * g * friction(l); // compute friction force from track
- if(l < .0001) // to prevent possible division by zero
- Fn = Ft = 0.0;
- else {
- Fn = -F * Ln/l; // compute components of force vector
- Ft = -F * Lt/l;
- }
- // compute power delivered:
- fb = (b < 0.0 ? -b : b) * (Ft * cosine + Fn * sine) - .9975*PM;
- }
- return b;
- }
-
- // Simulates the physics, moves the car by changing the state variables.
- // The angle "alpha" is the angle between the car's orientation angle and
- // its velocity vector. (like angle of attack of an aircraft)
- // Cornering force depends on alpha. (It is also thought of as a slip angle.)
- // "Slip" refers to wheel vs. track motion.
- // "vc" is the speed of the bottom of the wheel relative to the car.
- // This model is like a four-wheel drive car, since the forces are not
- // computed separately for front and rear wheels.
- void Car::move_car()
- {
- double D; // force on car from air, lb.
- double Fn, Ft; // normal & tangential components of track force vector
- double P; // power delivered to track, ft. lb. per sec.
- double v; // car's speed
- double Ln, Lt; // normal and tangential components of slip vector
- double l; // magnitude of slip vector, ft. per sec.
- double F; // force on car from track, lb.
- double x_a, y_a; // accelleration components in x & y directions
- double sine, cosine, separation, dx, dy;
- double pvec_x, pvec_y; // pointing vector of car (velocity vec + alpha)
- int i, other_seg;
- double dot, mag_prod, temp;
- double rel_xdot, rel_ydot; // velocity relative to other car being hit
-
- if(out) // This gets set if the car is stuck and off the track
- return;
- v = vec_mag(xdot, ydot); // the car's speed, feet/sec
- if(v > speed_max) // keep track of max speed
- speed_max = v;
- // limit how fast alpha can change, and its maximum value
- alpha = alpha_limit(prev_alpha, alpha);
- prev_alpha = alpha;
- sine = sin(alpha); cosine = cos(alpha); // alpha is angle of attack
- // don't allow reverse gear
- if(vc < 0.0) // This would be a reverse gear request
- if(v > REVERSE_GEAR_LIMIT) // if going too fast
- vc = 0.0; // make it maximum braking
-
- dead_ahead = 0; // will be set if there is a car more-or-less dead ahead
- for(i=0; i<car_count; i++) { // check for cars nearby or bumping
- if(i == which) continue; // ignore oneself
- other_seg = pcar[i]->seg_id; // only consider present and next segment
- if(!(seg_id == other_seg || incseg(seg_id) == other_seg))
- continue;
- dx = pcar[i]->x - x; // components of vector to other car
- dy = pcar[i]->y - y;
- separation = vec_mag(dx, dy); // distance to the other car
- if(separation > 2.5 * CARLEN) // ignore cars farther away than this
- continue;
- // Is there a car dead ahead? "dead ahead" means within 2.5 car lengths
- // AND within a few degrees of pointing vector (arcos(.94) == 20 deg.)
- // First compute pointing vector by rotating velocity vector by alpha:
- pvec_x = xdot * cosine - ydot * sine; // components of pointing vector:
- pvec_y = xdot * sine + ydot * cosine;
- // compute dot product, rel. position & pointing vectors:
- dot = pvec_x * dx + pvec_y * dy;
- mag_prod = separation * v; // p_vec and v vectors are same length
- if(dot > .94 * mag_prod) // test based on properties of dot product
- dead_ahead = 1; // there is a car more-or-less dead ahead
- if(separation > CARLEN) // If separation is greater than length,
- continue; // cars cannot be bumping.
- // cars might be bumping, test for that. Simplified test, assumes
- // cars are parallel. Also, only test for a car in front, as the
- // other car will test also, and find this car in front.
- if(v < .01) // to prevent division error just below
- continue;
- temp = dot / v; // dot/v is longitudinal component of separation
- if(temp > CARLEN || dot < 0.0)
- continue;
- if(separation - temp/separation > CARWID) // test for lateral separation
- continue;
- //The cars are bumping, reduce speed of rear car, and damage both;
- rel_xdot = xdot - pcar[i]->xdot;
- rel_ydot = ydot - pcar[i]->ydot;
- temp = rel_xdot * rel_xdot + rel_ydot * rel_ydot; //impact energy
- damage += .35 * temp;
- pcar[i]->damage += .2 * temp; // less damage to car in front
- xdot *= .8;
- ydot *= .8;
- }
-
- int it = 0;
- VC: // maybe loop to control power (we don't permit P > PM)
-
- Ln = -vc * sine; Lt = v - vc * cosine; // vector sum to compute slip vector
- l = vec_mag(Lt, Ln); // compute slip speed
- F = M * g * friction(l); // compute friction force from track
- D = DRAG_CON * v * v; // air drag force
- if(offroad) { // if the car is off the track,
- D = (0.6 + .008 * v) * M * g; // add a lot more resistance
- if(veryoffroad)
- D += 1.7 * M * g;
- }
- if(l < .0001) // to prevent possible division by zero
- Fn = Ft = 0.0;
- else {
- Fn = -F * Ln/l; // compute components of force vector
- Ft = -F * Lt/l;
- }
- // compute power delivered:
- P = (vc < 0.0 ? -vc : vc) * (Ft * cosine + Fn * sine);
-
- if(!it) {
- power_req = P/PM; // Tell the driver how much power it requested.
- if(P > PM) { // If the request was too high, reduce it to 100% pwr.
- ++it;
- vc = zbrent(sine, cosine, v, v * cosine, vc, .006);
- goto VC;
- }
- }
-
- // put some randomness in the magnitude of the traction force, F:
- if(Ft != 0.0) // This might be set to 0.0 above, in which case F will be zero
- temp = rfriction(l) * M * g / F; // ratio of new to original force
- else
- temp = 1.0;
-
- // compute centripetal and tangential acceleration components:
- cen_a = Fn * temp / M;
- tan_a = (Ft * temp - D) / M;
- if(offroad) { // this makes it stop when offroad and reversed
- damage += int(tan_a * tan_a + cen_a * cen_a) / 10;
- if(damage > 30000) {
- ++out_count;
- out = 1;
- return;
- }
- }
- if(v < .0001) // prevent division by zero
- adot = sine = cosine = 0.0;
- else {
- adot = cen_a / v; // angular velocity
- sine = ydot/v; cosine = xdot/v; // direction of motion
- }
- x_a = tan_a * cosine - cen_a * sine; // x & y components of acceleration
- y_a = cen_a * cosine + tan_a * sine;
- xdot += x_a * delta_time; // update the state vector:
- ydot += y_a * delta_time;
- x += (xdot + .5 * x_a * delta_time) * delta_time;
- y += (ydot + .5 * y_a * delta_time) * delta_time;
- if(v >= .0001)
- ang = atan2(ydot,xdot); // new orientation angle
- }
-
- void Car::draw_car(void) // Calls drawcar() twice, once to erase, once to draw
- {
- if(out) // Don't redraw a car that is out of the race.
- return;
- drawcar(prex, prey, prang, TRACK_COLOR, TRACK_COLOR); // erase old one
- drawcar(x, y, prang = ang+alpha, nose_color, tail_color); // draw new one
- prex = x; prey = y; // save these to erase car next time
- }
-
- // The Car constructor:
- Car::Car(colors use_these, double x_pos, double y_pos, double alf_ang, int i)
- {
- which = i;
- starting = 1;
- cntrl = drivers[i];
- out = lap_flag = seg_id = dead_ahead = 0;
- init_flag = damage = 0;
- laps = -1; // to become 0 crossing the finish line at start of race
- to_end = trackin[0].length;
- alpha = ydot = prang = ang = prev_alpha = 0.0;
- speed_avg = speed_max = 0;
- nose_color = use_these.nose;
- tail_color = use_these.tail;
- prex = x = x_pos;
- prey = y = y_pos;
- xdot = vc = STARTING_SPEED;
- ang = alf_ang;
- power_req = .9;
- data_ptr = (void*)new char[512];
- if(!no_display)
- draw_car(); // draw the new car on the screen
- }
-
- Car::~Car(void)
- {
- if(!no_display)
- drawcar(prex, prey, prang, TRACK_COLOR, TRACK_COLOR); // erase old car
- delete [] (char *)data_ptr;
- }
-
- // This function uses the current state of the car, and the current
- // track segment data, to compute the car's local situation as seen by
- // the driver. (see struct situation)
- situation Car::observe(rel_state* rel_vec_ptr)
- {
- double rad; // current radius
- double dx, dy, xp, yp;
- double sine, cosine, dot; // dot used for dot product of two vectors
- double temp;
- situation s;
- int nex_seg; // segment ID of the next segment
- double separation, dxdot, dydot;
- int i, k, m, other_seg;
- // These two arrays describe the three closest cars:
- // element 0 is for the closest, element 1 next, element two after that.
- int closest[] = { 999, 999, 999 }; // their ID's, initially too big
- double how_close[] = { 1e5, 1e5, 1e5 }; // initially very large distances
- int flag = 1; // controls possible repetition of calculations due to
- // completion of a lap.
- if(out) // This gets set if the car is stuck and off the track
- return(s);
-
- s.nearby = rel_vec_ptr;
- s.v = vec_mag(xdot, ydot); // the actual speed
- s.dead_ahead = dead_ahead; // copy the value set by move_car()
- s.power_req = power_req; // copy the value set by move_car()
- // Computations are base on the right wall of track, which is described
- // by trackout[], except radii are based on the smaller, or inner, curve.
-
- while(flag) { // This loop repeats only when a segment boundary is crossed,
- // in which case it repeats once:
-
- sine = sin(temp = trackout[seg_id].beg_ang); // track direction
- cosine = cos(temp);
- if((nex_seg = seg_id + 1) == NSEG) // which segment is next
- nex_seg = 0;
- s.nex_len = trackout[nex_seg].length; // length and radius
- s.nex_rad = trackin[nex_seg].radius; // of next segment
- if(s.nex_rad < 0.0) // always use smaller radius
- s.nex_rad = trackout[nex_seg].radius;
- if(++nex_seg == NSEG)
- nex_seg = 0;
- s.after_rad = trackin[nex_seg].radius; // and the one after that
- if(s.after_rad < 0.0) // always use smaller radius
- s.after_rad = trackout[nex_seg].radius;
- s.cur_len = trackout[seg_id].length; // copy these two fields:
- s.cur_rad = rad = trackin[seg_id].radius; // rt. turn will use trackout
- if(seg_id != 0) // This is used to tell when the finish line is
- lap_flag = 0; // crossed, thus completing each lap.
-
- if(rad == 0) { // if current segment is straight,
- // xp and yp locate the car with respect to the beginning of the right
- // hand wall of the straight segment. calculate them:
- dx = x - trackout[seg_id].beg_x;
- dy = y - trackout[seg_id].beg_y;
- xp = dx * cosine + dy * sine;
- yp = dy * cosine - dx * sine;
- s.to_rgt = yp; // fill in to_rgt and to_end:
- s.to_end = s.cur_len - xp;
-
- if(seg_id == 0)
- if(xp > FINISH * s.cur_len && !lap_flag) {
- lap_flag = 1; // This means the line crossing was noted.
- if(++laps == lap_count)
- ++done_count;
- if(!no_display)
- lapper(which, laps); // display the new lap count
- lap[which] = 1;
- if(laps == 0) // record when the starting line is crossed:
- start_time = time_count;
- else // update overall average speed:
- speed_avg = length * laps / (time_count-start_time);
- }
-
- // here we make sure we are still in the same segment:
- if(s.to_end <= 0.0) { // see if a lap has been completed,
- if(++seg_id == NSEG)
- seg_id = 0;
- continue; // repeat the loop in context of next segment
- }
- s.to_lft = width - yp; // fill in to_lft & cur_rad:
- s.cur_rad = 0.0;
- s.vn = ydot * cosine - xdot * sine; // compute cross-track speed
- s.backward = (xdot * cosine + ydot * sine < 0.0);
- }
- else if(rad > 0.0) { // when current segment is a left turn:
- dx = x - trackout[seg_id].cen_x; // compute position relative to center
- dy = y - trackout[seg_id].cen_y;
- temp = atan2(dy, dx); // this is the current angular position
- s.to_end = trackout[seg_id].end_ang - temp - PI/2.0; // this is an angle
- if(s.to_end > 1.5 * PI)
- s.to_end -= 2.0 * PI;
- else if(s.to_end < -.5 * PI)
- s.to_end += 2.0 * PI;
- if(s.to_end <= 0.0) { // Handle segment crossing:
- if(++seg_id == NSEG)
- seg_id = 0; // going from last segment to 1st
- continue;
- }
- s.to_lft = vec_mag(dx, dy) - rad;
- s.to_rgt = width - s.to_lft;
- s.vn = (-xdot * dx - ydot * dy)/vec_mag(dx, dy); // a trig thing
- s.backward = (ydot * dx - xdot * dy < 0.0);
- }
- else {
- s.cur_rad = rad = trackout[seg_id].radius; // rt. turn needs trackout
- dx = x - trackout[seg_id].cen_x; // compute position relative to center
- dy = y - trackout[seg_id].cen_y;
- temp = atan2(dy, dx); // this is the current angular position
- s.to_end = -trackout[seg_id].end_ang + temp - PI/2.0; // this is an angle
- if(s.to_end < -.5 * PI)
- s.to_end += 2.0 * PI;
- else if(s.to_end >= 1.5 * PI)
- s.to_end -= 2.0 * PI;
- if(s.to_end <= 0.0) { // Handle segment transistion:
- if(++seg_id == NSEG)
- seg_id = 0;
- continue;
- }
- s.to_rgt = vec_mag(dx, dy) + rad;
- s.to_lft = width - s.to_rgt;
- s.vn = (xdot * dx + ydot * dy)/vec_mag(dx, dy); // a trig thing
- s.backward = (xdot * dy - ydot * dx < 0.0);
- }
-
- // If we get this far, we do not repeat the big loop.
- flag = 0;
- } // end while(flag) loop
- if(s.backward) {
- to_end = s.to_end;
- offroad = (s.to_lft < 0.0 || s.to_rgt < 0.0); // maybe set offroad flag
- veryoffroad = (s.to_lft < -width || s.to_rgt < -width); // maybe veryoffroad
- return s;
- }
-
- offroad = (s.to_lft < 0.0 || s.to_rgt < 0.0); // maybe set offroad flag
- veryoffroad = (s.to_lft < -width || s.to_rgt < -width); // maybe veryoffroad
- to_end = s.to_end; // fill in this field in Car object
-
- // find three closest cars in front:
- // "front", here, is direction of velocity vector, not pointing vector.
- for(i=0; i<car_count; i++) { // check for cars nearby
- if(i == which) continue; // ignore oneself
- other_seg = pcar[i]->seg_id; // only consider present and next segment
- if(!(seg_id == other_seg || incseg(seg_id) == other_seg))
- continue;
- dx = pcar[i]->x - x; // components of vector to other car
- dy = pcar[i]->y - y;
- separation = vec_mag(dx, dy); // distance to the other car
- // compute dot product, rel. position & velocity vectors:
- dot = xdot * dx + ydot * dy;
- if(dot < 0.0)
- continue; // Ignore cars behind you.
- for(k=0; k<3; k++) {
- if(separation < how_close[k]) {
- for(m = 2; m > k; m--) {
- closest[m] = closest[m-1];
- how_close[m] = how_close[m-1];
- }
- closest[k] = i;
- how_close[k] = separation;
- break;
- }
- }
- }
- // now compute local relative state vector for those three cars:
- for(k=0; k<3; k++) {
- if(closest[k] > car_count) { // This is when there are less than 3
- for(; k<3; k++)
- s.nearby[k].who = 999;
- break;
- }
- i = closest[k];
- dx = pcar[i]->x - x; // components of relative position vector
- dy = pcar[i]->y - y;
- dxdot = pcar[i]->xdot - xdot; // components of relative velocity
- dydot = pcar[i]->ydot - ydot;
- // the relative vectors must be found wrt to velocity vector frame
- // we need sine and cosine of rotation of axes:
- if(s.v > 1e-7) { // prevent division by 0
- sine = - xdot / s.v; // direction of velocity
- cosine = ydot / s.v; // wrt the x axis.
- }
- else {
- sine = -1.0;
- cosine = 0.0;
- }
- s.nearby[k].who = i;
- s.nearby[k].rel_x = cosine * dx + sine * dy;
- s.nearby[k].rel_y = cosine * dy - sine * dx;
- s.nearby[k].rel_xdot = cosine * dxdot + sine * dydot;
- s.nearby[k].rel_ydot = cosine * dydot - sine * dxdot;
- }
-
- return s;
- }
-
- // create a default situation vector to pass to control() during initializaton
- situation fill_situation(rel_state* rel_state_vec_ptr)
- {
- situation result;
-
- result.cur_rad = 0.0;
- result.cur_len = 1.0;
- result.to_lft = 1.0;
- result.to_rgt = 1.0;
- result.to_end = 1.0;
- result.v = STARTING_SPEED;
- result.vn = 0.0;
- result.nex_len = 1.0;
- result.nex_rad = 1.0;
- result.after_rad = 0.0;
- result.power_req = 1.0;
- result.dead_ahead = 0;
- result.backward = 0;
- result.nearby = rel_state_vec_ptr;
- for(int k=0; k<3; k++)
- result.nearby[k].who = 999;
-
- return result;
- }
-
- // gets the names from the robot drivers and stores them in RAM.
- // nam_ptr[] is the array of pointers to the names.
- // some elements of nam_ptr[] may point to the same place; others may be 0;
- void get_names(void)
- {
- int i, j;
- situation s; // to pass data from observe() to control()
- rel_state rel_state_vec[3]; // needed by fill_situation() function
- con_vec (*func_ptr)(situation);
-
- for(i=0; i<MAXCARS; i++) {
- glob_name[0] = 0; // to check later to see if driver filled it
- // fill in s to avoid div. by zero in driver.
- s = fill_situation(rel_state_vec);
- // The first call to a robot driver may put its name in glob_name.
- func_ptr = drovers[i];
- (*func_ptr)(s);
- if(glob_name[0] != 0) { // add new name to nam_ptr[] array:
- nam_ptr[i] = new char[strlen(glob_name) + 1];
- strcpy(nam_ptr[i], glob_name);
- *(nam_ptr[i] + 8) = '\0'; // chop any names greater than 8 chars.
- }
- else // if no name was given, see if there is
- for(j=i-1; j>=0; j--) // already a name for that driver:
- if(drovers[i] == drovers[j]) {
- nam_ptr[i] = nam_ptr[j];
- break;
- }
-
- }
- }
-
- // Will put a random permutation of the robot drivers in "drovers[]"
- // into the "drivers[]" array.
- void pick_random_order(void)
- {
- int selected[MAXCARS]; // marks the points as they are picked
- int racers[MAXCARS]; // the random permutation goes here
- int i, j, k, count;
-
- for(i=0; i<car_count; i++) { // initialize selected[] to all zeroes:
- selected[i] = 0; // (meaning no points are picked)
- }
-
- // for each element of rptr, j is no. of alternates
- for(j=car_count; j>0; j--) { // remaining. here we pick one of them:
- k = (j > 1) ? ((int)random(j)) : (0); // k chooses among the remaining pts.
- for(i=0, count=0; i<car_count; i++)
- if(!selected[i]) // if not already picked,
- if(count++ == k) {
- selected[i] = 1; // marks this point as taken
- racers[j-1] = i; // puts it into the genome
- }
- }
- // now copy the selected members of drovers[] into drivers[],
- // and the same for their colors & names:
- for(i=0; i<car_count; i++) {
- drivers[i] = drovers[racers[i]];
- car_colors[i] = car_clrs[racers[i]];
- namptr[i] = nam_ptr[racers[i]];
- }
-
- }
-
- // copy drovers[] into drivers[], and the same for their colors & names:
- void copy_drivers(void)
- {
- int i;
- for(i=0; i<car_count; i++) {
- drivers[i] = drovers[i];
- car_colors[i] = car_clrs[i];
- namptr[i] = nam_ptr[i];
- }
-
- }
-
- void reverse_order(void)
- {
- colors temp_color;
- con_vec(*temp_driver)(situation);
- char *temp_ptr;
- for(int i=0; i<car_count/2; i++) {
- temp_driver = drivers[i];
- drivers[i] = drivers[car_count - 1 - i];
- drivers[car_count - 1 - i] = temp_driver;
- temp_ptr = namptr[i];
- namptr[i] = namptr[car_count - 1 - i];
- namptr[car_count - 1 - i] = temp_ptr;
- temp_color = car_colors[i];
- car_colors[i] = car_colors[car_count - 1 - i];
- car_colors[car_count - 1 - i] = temp_color;
- }
- }
-
- main(int argc, char* argv[]) // 1st arg is no. of cars in race
- { // 2nd arg is no. of laps to race
- int i, j, ml;
- double x, y, dx, dy; // used only for initial positions of cars
- situation s; // to pass data from observe() to control()
- int num_disp; // how many cars to display in leaders area of scoreboard
- int starting; // indicates the race is just starting
- rel_state rel_state_vec[3]; // contains up to 3 relative state vectors
-
- glob_name = new char[33];
- strcpy(glob_name, "Not yet filled in, 32 characters");
-
- get_names(); // call all the robot drivers to fill in the nam_ptr[] array.
-
- // get argument values into car_count and lap_count global variables
- get_args(argc, argv); // sets car_count and lap_count
- num_disp = (car_count < 5) ? car_count : 5; // show up to 5
- if(no_display)
- real_speed = 0;
-
- // setup the graphics display, also compute track data:
- graph_setup();
-
- randomizer();
- report_overall(car_count, lap_count, nam_ptr);
- // Main loop - Once through here for every complete race
- for(ml=0; ml<race_count; ++ml) {
- done_count = 0; // incremented by each car that finishes the race
- out_count = 0; // incremented by each car that crashes
- time_count = 0.0; // elapsed time, seconds
- starting = 1;
-
- if(keep_order)
- copy_drivers();
- else
- if(!(ml%2))
- pick_random_order(); // arrange the starting positions
- else
- reverse_order(); // reverse positions for 2nd race
-
- // create cars (instances of car class)
- dy = width/4; // the dy and dx stuff is for arranging
- dx = 2.5 * CARLEN; // the cars in their starting positions
- x = trackout[0].beg_x - dx; // We assume that segment
- for(i=0; i<car_count; i++) { // 0 is straight, and
- if(!(i%4)) { // parallel to the x axis.
- y = trackout[0].beg_y + dy/2; // Cars start on seg. 0.
- x += dx;
- }
- else
- y += dy;
- pcar[i] = new Car(car_colors[i], x, y, 0, i); // This is the key part!
- }
- if(!car_count) { //When zero cars are requested, we only show the track.
- if(no_display) {
- cout << "Zero cars were requested." << endl;
- exit(0);
- }
- get_ch(); // this executes only when 0 cars are requested
- resume_normal_display();
- exit(0);
- }
-
- // Put up the scoreboard and leader board:
- if(!no_display)
- scoreboard();
-
- done_count = 0;
- one_tick(1);
- order = new int[car_count]; //setup the order[] array:
- new_data = new int[num_disp]; //setup the new_data[] array:
- for(i=0, j=car_count-1; i<car_count; i++, j--) { // initialize order[] array
- order[i] = j; // must correspond with initial track positions
- lap[i] = 1;
- }
- if(!no_display)
- if(get_ch() == ESC) // Don't actually start the race until someone hits a key.
- break; // Any key except ESC begins the race.
- // BEGIN THE RACE!
- // Note that the lap count on the scoreboard is updated by the
- // observe() function, which call lapper() to do that.
- if(no_display)
- cout << "Beginning race " << ml+1 << endl;
- while(1) { // main loop of the race:
- for(i=0; i<car_count; i++) { // for each car:
- s = pcar[i]->observe(rel_state_vec); // compute its local situation
- pcar[i]->control(s); // compute a control vector
- }
- for(i=0; i<car_count; i++) { // for each car:
- pcar[i]->move_car(); // update state of car
- }
- if(!no_display)
- for(i=0; i<car_count; i++) // for each car:
- pcar[i]->draw_car(); // update screen image of car
- time_count += delta_time; // Advance the simulated time.
- sortem(num_disp); // maintains the order[] and new_data[] arrays
- // For any line flagged by new_data[], update that line in "LEADERS" area:
- for(i=0; i<num_disp; i++)
- if(starting || new_data[i] || lap[order[i]]) {
- lap[order[i]] = 0;
- if(!no_display)
- leaders(i);
- }
- if(!no_display)
- refresh_finish_line();
- if(kb_hit()) // respond to the keyboard if its hit
- if(get_ch() == ESC) // ESC key will end the race.
- break;
- // check for race over:
- if(done_count >= 2 || done_count >= car_count - out_count)
- break;
- if(real_speed)
- one_tick(0); // wait here till the next clock tick
- starting = 0; // the race is on!
- }
- if(no_display)
- cout << "End of race " << ml+1 << endl;
-
- report_results(ml+1, order, namptr, pcar);
-
- if(!no_display)
- if(get_ch() == ESC) // Continue showing end of race until someone hits a key.
- break;
-
- for(i=0; i<car_count; i++) { // delete all the car objects
- delete pcar[i];
- }
- } // End of Main Loop.
-
- /* clean up, end graphics, back to normal */
- resume_normal_display();
- RAM_report();
- return 0; // all done with execution, return to DOS.
- }
-
-